#include <stdio.h>
#include <stdlib.h>
#include <math.h>

//#define _USE_PRED

int data_size;
double *data;
double *filtered;
char *data_file;


void
read_args(int argc, char **argv)
{
	if (argc < 3)
		exit(printf("Use %s <data-file> <num-points>\n", argv[0]));

	data_file = argv[1];
	data_size = atoi(argv[2]);
}


void
initialize()
{
	data = (double *) calloc (data_size, sizeof(double));
	filtered = (double *) calloc (data_size, sizeof(double));
}


void
read_data()
{
	int i;
	FILE *f;

	if ((f = fopen(data_file, "r")) == NULL)	
		exit(printf("File '%s' not exists\n", data_file));

	for (i = 0; i < data_size; i++)
		fscanf(f, "%lf", &(data[i]));

	fclose(f);
}


double 
compute_initial_mean()
{
	int i;
	double initial_mean;

	initial_mean = 0;

	for (i = 0; i < 10; i++)
		initial_mean += data[i];

	initial_mean /= 10.0;

	return initial_mean;
}


double 
compute_initial_covar(double initial_mean)
{
	int i;
	double initial_covar;

	initial_covar = 0;

	for (i = 0; i < 10; i++)
		initial_covar += pow(data[i] - initial_mean, 2);

	initial_covar /= 10.0;

	return initial_covar;
}


void
kalman()
{
	int i;
	double mean, covar;
	double transition, measure;
	double pred_mean, pred_covar;
	double A, B, C, R, Q, K;

	A = 1.0; // how much of the state (t - 1) must be transmited to state (t)
	B = 1.0; // how much of the transition must be applied from state (t - 1) to state (t)
	C = 1.0; // transform from state universe to measure universe
	R = 1.0; // error in the prediciton 
	Q = 1.0; // error in the measure 

	// This is a smart way of initializing the mean and the covar matrix. 
	// mean = compute_initial_mean();
	// covar = compute_initial_covar(mean);

	// The fairiest way should be initialize the covar with a high value and the mean with the first data.
	mean = data[0];
	covar = 100.0;

	double v = 0.0;
	double corr_t_2, corr_t_1;

	corr_t_2 = data[0];
	corr_t_1 = data[0];

	for (i = 0; i < data_size; i++)
	{
#ifdef _USE_PRED
		if (i > 2)
		{
			v = (corr_t_1 - corr_t_2) / 2.0;
			transition = v + mean;

			// OBS: Pelo codigo do Fernando Awat, quando so um sensor eh usado e/ou nao existe 
			// modelo de transicao de estados somente a correcao deve ser feita!
			pred_mean = A * mean + B * transition;
			// pred_mean = mean;
			pred_covar = A * covar * A + R;
			// pred_covar = covar;
		}
#else
		pred_mean = mean;
		pred_covar = covar;
#endif

		measure = data[i];
		K = pred_covar * C * pow(C * pred_covar * C + Q, -1);
		
		mean = pred_mean + K * (measure - C * pred_mean);
		covar = (1 - K * C) * pred_covar;
		fprintf(stderr, "%lf\n", covar);

		filtered[i] = mean;

		corr_t_2 = corr_t_1;
		corr_t_1 = mean;
	}
}


void
show_output()
{
	int i;

	for (i = 0; i < data_size; i++)
		printf("%lf\n", filtered[i]);
}


int
main(int argc, char **argv)
{
	read_args(argc, argv);

	initialize();
	read_data();
	kalman();
	show_output();

	return 0;
}

